package com.robby.KUKAlwrhi;

import java.net.ConnectException;
import java.util.Hashtable;

import coppelia.FloatWA;
import coppelia.IntW;
import coppelia.remoteApi;

/**
 * JVRep allows to interact with the V-Rep Simulation Environment. 
 * @author robb19y
 *
 */
public class JVRep {
	private static final int PORT = 19997;
	private static final String HOST = "127.0.0.1";
	
	private remoteApi vrep;
	private int clientID;
	
	private Hashtable<Integer, IntW> models;
	
	/**
	 * Constructs an instance of JVRep.
	 * Default values for remote connection are used. (Host - 127.0.0.1 , Port - 19997)
	 * @throws Exception in case the remote server can not be reached. 
	 */
	public JVRep() throws Exception{
		super();
		//Initialize V-Rep connection.
		vrep = new remoteApi();
		//vrep.simxFinish(-1);
		clientID = vrep.simxStart(HOST,PORT,true,true,5000,5);
		if(clientID == -1){
			throw new ConnectException();
		}		
		init();
	}
	
	/**
	 * Constructs an instance of JVRep. Values for remote connection must be specified.
	 * @param host - host of V-Rep service.
	 * @param port - port of V-Rep service.
	 * @throws Exception in case the remote server can not be reached. 
	 */
	public JVRep(String host,int port) throws Exception{
		super();
		//Initialize V-Rep connection.
		vrep = new remoteApi();
		vrep.simxFinish(-1);
		clientID = vrep.simxStart(host,port,true,true,5000,5);
		if(clientID == -1){
			throw new ConnectException();
		}		
		init();
	}
	
	private void init(){
		models = new Hashtable<Integer, IntW>();
		models.put(-1, new IntW(-1));
	}
	
	/**
	 * Starts the simulator.
	 */
	public void startSimulation(){
		vrep.simxStartSimulation(clientID, remoteApi.simx_opmode_oneshot_wait);
	}
	
	/**
	 * Stop the simulator.
	 */
	public void stopSimulation(){
		vrep.simxStopSimulation(clientID, remoteApi.simx_opmode_oneshot_wait);
	}
	
	/**
	 * Load a model in the simulator.
	 * @param identifier - identifier of the model. (A number used for call this method from youBot Simulink toolbox)
	 * @param modelFilePath - complete file path of the model.
	 */
	public void loadModel(int identifier,String modelFilePath){
		IntW handle = new IntW(0);
		vrep.simxLoadModel(clientID, modelFilePath, 0, handle, remoteApi.simx_opmode_oneshot_wait);
		models.put(identifier, handle);
	}
	
	/**
	 * Removes an object from the simulator.
	 * @param identifier
	 */
	protected void removeObject(int identifier){
		vrep.simxRemoveObject(clientID, models.get(identifier).getValue(), remoteApi.simx_opmode_oneshot_wait);
		models.remove(identifier);		
	}
	
	/**
	 * Use this method to initialize a model already present in the simulator and not loaded with JVRep.
	 * @param identifier - identifier of the model. (A number used for call this method from youBot Simulink toolbox)
	 * @param modelName - name of the model.
	 */
	public void setModelIdentifier(int identifier,String modelName){
		IntW handle = new IntW(0);
		vrep.simxGetObjectHandle(clientID, modelName, handle, remoteApi.simx_opmode_oneshot_wait);
		models.put(identifier, handle);
	}
	
	/**
	 * Returns the actual state of the model (position and orientation).
	 * @param identifier - identifier of the model.
	 * @param identifierRelativeReference - object used for reference. (-1 for Absolute frame).
	 * @return an array representing the model state [x y z roll pitch yaw].
	 */
	public double[] getObjectState(int identifier,int identifierRelativeReference){
		FloatWA pos = new FloatWA(3);
		vrep.simxGetObjectPosition(clientID, models.get(identifier).getValue(), models.get(identifierRelativeReference).getValue(), pos, remoteApi.simx_opmode_streaming);
		double[] res = new double[6];
		res[0] = (double) pos.getArray()[0];
		res[1] = (double) pos.getArray()[1];
		res[2] = (double) pos.getArray()[2];
		
		vrep.simxGetObjectOrientation(clientID, models.get(identifier).getValue(), models.get(identifierRelativeReference).getValue(), pos, remoteApi.simx_opmode_streaming);
		res[3] = (double) pos.getArray()[0];
		res[4] = (double) pos.getArray()[1];
		res[5] = (double) pos.getArray()[2];
		return res;
		
	}
	
	/**
	 * Returns the actual state of the model (position and orientation).
	 * @param identifier  - identifier of the model.
	 * @return an array representing the model state [x y z roll pitch yaw].
	 */
	public double[] getObjectState(int identifier){
		return getObjectState(identifier,-1);
	}
	
	/**
	 * Set position and orientation of a model.
	 * @param identifier - identifier of the model.
	 * @param identifierRelativeReference - object used for reference. (-1 for Absolute frame).
	 * @param state - array representing the desidered model state [x y z roll pitch yaw].
	 */
	public void setObjectState(int identifier,int identifierRelativeReference,double[] state){
		FloatWA pos = new FloatWA(3);
		pos.setValue(0, (float) state[0]);
		pos.setValue(1, (float) state[1]);
		pos.setValue(2, (float) state[2]);
		vrep.simxSetObjectPosition(clientID, models.get(identifier).getValue(), models.get(identifierRelativeReference).getValue(), pos, remoteApi.simx_opmode_oneshot_wait);
		pos.setValue(0, (float) state[3]);
		pos.setValue(1, (float) state[4]);
		pos.setValue(2, (float) state[5]);
		vrep.simxSetObjectOrientation(clientID, models.get(identifier).getValue(), models.get(identifierRelativeReference).getValue(), pos, remoteApi.simx_opmode_oneshot_wait);
	
	}
	
	/**
	 * Set position and orientation of a model.
	 * @param identifier - identifier of the model.
	 * @param state - array representing the desidered model state [x y z roll pitch yaw].
	 */
	public void setObjectState(int identifier,double[] state){
		setObjectState(identifier,-1,state);
	}
}
